#include"rclcpp/rclcpp.hpp"
#include"rclcpp_action/rclcpp_action.hpp"
#include "base_interfaces_demo/action/progress.hpp"
using base_interfaces_demo::action::Progress;
using std::placeholders::_1;
using std::placeholders::_2;
class ActionServer:public rclcpp::Node
{
public:
  ActionServer():Node("action_server_node_cpp")
  {
    RCLCPP_INFO(this->get_logger(),"动作服务节点已创建");
// rclcpp_action::Server<ActionT>::SharedPtr 
// create_server<ActionT, NodeT>
// (NodeT node, 
// const std::string &name, 
// rclcpp_action::Server<ActionT>:: GoalCallback handle_goal, 
// rclcpp_action::Server<ActionT>::CancelCallback handle_cancel, 
// rclcpp_action::Server<ActionT>::AcceptedCallback handle_accepted, 
    this->_action_server=rclcpp_action::create_server<Progress>
    (this,
     "get_sum",
     std::bind(&ActionServer::handle_goal,this,_1,_2),
     std::bind(&ActionServer::handle_cancel,this,_1),
     std::bind(&ActionServer::handle_accepted,this,_1)
    );
  }
private:  
    rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID &uuid,const  std::shared_ptr<const Progress::Goal>goal)
    {
      (void) uuid;
      int64_t goal_num=goal->num;
      if(goal_num<=1)
      {
        RCLCPP_ERROR(this->get_logger(),"请输入大于1的整形数字！！！");
        return rclcpp_action::GoalResponse::REJECT;
      }
      else
      {
        RCLCPP_INFO(this->get_logger(),"接收到目标数字：%ld",goal_num);
      }  
      return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
    }
  
    rclcpp_action::CancelResponse handle_cancel(std::shared_ptr<rclcpp_action::ServerGoalHandle<Progress>>servergoalhandle)
    {
      (void) servergoalhandle;
      RCLCPP_INFO(this->get_logger(),"接收到任务取消请求！！！");
      return rclcpp_action::CancelResponse::ACCEPT;
    }
    void execute(std::shared_ptr<rclcpp_action::ServerGoalHandle<Progress>>goal_handle)
    {
      auto feedback=std::make_shared<Progress::Feedback>();
      auto result=std::make_shared<Progress::Result>();
      int64_t num=goal_handle->get_goal()->num;
      int64_t sum=0;
      rclcpp::Rate rate(1.0);
      for(int i=1;i<=num;i++)
      {
        sum+=i;
        if(goal_handle->is_canceling())
        {
          result->sum=sum;
          goal_handle->canceled(result);
          RCLCPP_INFO(this->get_logger(),"计算取消，值为：%ld",result->sum);
          return;
        }
        feedback->progress=(double_t)i/num;
        goal_handle->publish_feedback(feedback);
        RCLCPP_INFO(this->get_logger(),"计算进度：%.2f",feedback->progress);
        rate.sleep();
      }
      if(rclcpp::ok())
      {
        result->sum=sum;
        goal_handle->succeed(result);
        RCLCPP_INFO(this->get_logger(),"计算完毕，最终值为：%ld",result->sum);
      }  
    }
    void handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<Progress>>goal_handle)
    {
      std::thread(bind(&ActionServer::execute,this,goal_handle)).detach();
    }
    rclcpp_action::Server<Progress>::SharedPtr _action_server;    
};
int main(int argc,char *argv[])
{
  rclcpp::init(argc,argv);
  RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"程序已启动");
  rclcpp::spin(std::make_shared<ActionServer>());
  RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"程序已退出");
  rclcpp::shutdown();
  return 0;
}
